#include "drivetrain.h"
#include "message.pb.h"
#include "messages_robocup_ssl_wrapper.pb.h"
#include "messages_robocup_ssl_geometry.pb.h"
#include "messages_robocup_ssl_detection.pb.h"
#include <WarbotsMsg/velocityMsg.h>

// Drivetrain controller object
drivetrain dt;
WarbotsMsg::manualCtrlMsg mMsg;

/*----------- function prototype ---------------*/
void cmd_msg_handler(Message* msg);
void bot_wrapper_msg_handler(SSL_DetectionRobot* msg);
void manual_control_Callback(const WarbotsMsg::manualCtrlMsg& msg);

void cmd_msg_handler(Message* msg)
{
   switch( msg->message_type() )
   { 
      case Message::NOP:
         break;
      case Message::MOVE:
         waypt_t nWaypts[msg->move_data().waypoints().points_size()];
         for(int i=0; i<msg->move_data().waypoints().points_size(); i++)
         {
            nWaypts[i].pos.x = (double)(msg->move_data().waypoints().points(i).x());
            nWaypts[i].pos.y = (double)(msg->move_data().waypoints().points(i).y());
            nWaypts[i].heading = (double)(msg->move_data().expected_orientation())*PI/180.0;
            nWaypts[i].Tstamp = (double)(msg->move_data().power());
         }
         dt.set_traj(nWaypts, msg->move_data().waypoints().points_size(), false);
         break;
      case Message::GET_BALL:
         break;
      case Message::STOP:
         dt.stop();
         break;
      case Message::BOT_STATE_UPDATE:
         break;
      case Message::BOT_STATUS:
         break;
      default:
         break;
   }
}

void bot_wrapper_msg_handler(SSL_WrapperPacket* msg)
{
   WarbotsMsg::posUpdateMsg pMsg;
   pMsg.x = msg->detection().TEAM_COLOUR(PLAYER_ID).x();
   pMsg.y = msg->detection().TEAM_COLOUR(PLAYER_ID).y();
   pMsg.theta = msg->detection().TEAM_COLOUR(PLAYER_ID).orientation();
   //dt.vehicle_posUpdate(pMsg);
}

void manual_control_Callback(const WarbotsMsg::manualCtrlMsg& msg)
{
   mMsg = msg;
}

void test_msg()
{
   // MOVE msg
   Message MoveMsg;
   MoveMsg.set_message_type(Message::MOVE);
   MoveData* mvdata = MoveMsg.mutable_move_data();
   Waypoints* waypt = mvdata->mutable_waypoints();
   Point *pt_1 = waypt->add_points();
   pt_1->set_x(5);
   pt_1->set_y(6);
   Point *pt_2 = waypt->add_points();
   pt_2->set_x(10);
   pt_2->set_y(20);
   mvdata->set_expected_orientation(180);
   mvdata->set_power(10);
   cmd_msg_handler(&MoveMsg);

   // Stop msg
   Message StopMsg;
   StopMsg.set_message_type(Message::STOP);
   //cmd_msg_handler(&StopMsg);
}

int main(int argc, char **argv)
{
   // Verify that the version of the library that we linked against is
   // compatible with the version of the headers we compiled against.
   GOOGLE_PROTOBUF_VERIFY_VERSION;

   ros::init(argc, argv, "drive_controller");
   ros::start();
   ros::Rate loop_rate(DRIVE_CTRL_LOOP_RATE);
   ros::Rate poll_rate(POLL_RATE);
   ros::NodeHandle posFeed, mCtrl, n_manual;
   ros::Publisher ref_mRot_spd;
   ros::Subscriber pos_sub;
   ref_mRot_spd = mCtrl.advertise<WarbotsMsg::velocityMsg>("SetSpd", 1000);
   pos_sub = posFeed.subscribe("position_update", 1, &drivetrain::vehicle_posUpdate, &dt);
   ros::Subscriber manual_sub = n_manual.subscribe("manual_control", 1, manual_control_Callback);
   WarbotsMsg::velocityMsg vMsg;
   double set_spd[NUM_OF_WHEELS];

   while( ref_mRot_spd.getNumSubscribers() == 0 )
   {
      poll_rate.sleep();
   }

   dt.reset();
   dt.start();

   //test_msg();  // sending the test msg
  
   while( ros::ok() )
   {
#ifndef MANUAL_CONTROL
      if(dt.getMode() == RUNNING)
      {
         dt.set_wheelRotSpd(set_spd);
         vMsg.v1 = set_spd[0];
         vMsg.v2 = set_spd[1];
         vMsg.v3 = set_spd[2];
         vMsg.v4 = set_spd[3];
         ref_mRot_spd.publish(vMsg);
      }
#else
      dt.manual_control(mMsg, set_spd);
      vMsg.v1 = set_spd[0];
      vMsg.v2 = set_spd[1];
      vMsg.v3 = set_spd[2];
      vMsg.v4 = set_spd[3];
      ref_mRot_spd.publish(vMsg);
#endif
      ros::spinOnce();
      loop_rate.sleep();
   }

   // Optional:  Delete all global objects allocated by libprotobuf.
   google::protobuf::ShutdownProtobufLibrary();


   return 0;
}
